//------------------------------------------------------------------------------
//  <copyright file="ManagedKukaFriThunk.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------
#include "stdafx.h"
#include "FRIdispatcher.h"
#include "ManagedKukaFriThunk.h"
#include <Windows.h>

using namespace Microsoft::Robotics::Hardware::KukaLBR;

/// <summary> Initializes a new instance of the <c>ManagedKukaFriThunk</c> class with callbacks to a managed FRI. </summary>
/// <param name="managedFRIConnection"> Managed FRI handler </param>
ManagedKukaFriThunk::ManagedKukaFriThunk(IManagedFRI^ managedFRIConnection)
{
    this->SetDispatcherHandlers(managedFRIConnection);
}

/// <summary> class destructor </summary>
ManagedKukaFriThunk::~ManagedKukaFriThunk()
{
	// we do not delete pFRIconnection here as it is expected to be owned
	// by the controller object that uses it.
    if(nullptr != this->delegateHandles)
    {
        for (int i = 0 ; i < this->delegateHandles->Count ; ++i)
        {
            if (true == this->delegateHandles[i].IsAllocated)
            {
                this->delegateHandles[i].Free();
            }
        }
    }
}

/// <summary> 
/// Create the call backs. The friConnection is expected to be owned 
/// and deleted by the controller object that will use it.
/// </summary>
void ManagedKukaFriThunk::SetDispatcherHandlers(IManagedFRI^ managedFRIConnection)
{
    this->managedFRIConnection = managedFRIConnection;

    delegateHandles = gcnew List<GCHandle>();

    this->pFRIconnection = new FRIdispatcher(
                            this->CreateCallBack0(gcnew Parameter0Delegate(this, &ManagedKukaFriThunk::InitializeFRI)),
                            this->CreateCallBack0(gcnew Parameter0Delegate(this, &ManagedKukaFriThunk::CloseFRI)),
                            this->CreateCallBack0(gcnew Parameter0Delegate(this, &ManagedKukaFriThunk::WaitOnFRIConnect)),
                            this->CreateCallBack0(gcnew Parameter0Delegate(this, &ManagedKukaFriThunk::DoNothing)),
                            this->CreateCallBackPPD1(gcnew ParameterPPD1Delegate(this, &ManagedKukaFriThunk::GetFRIRequest)),
                            this->CreateCallBackPD1(gcnew ParameterPD1Delegate(this, &ManagedKukaFriThunk::DoPositionControl)),
                            this->CreateCallBackPD4(gcnew ParameterPD4Delegate(this, &ManagedKukaFriThunk::DoJointImpedanceControl)),
                            this->CreateCallBackPD4(gcnew ParameterPD4Delegate(this, &ManagedKukaFriThunk::DoCartesianImpedanceControl)),
							this->CreateCallBackPD1I1(gcnew ParameterPD1I1Delegate(this, &ManagedKukaFriThunk::DoKrlCartesianImpedanceControl)));
}

/// <summary> Helper function to create a zero parameter callback. </summary>
/// <param name="delgate0">Call back function </param>
FRICALLBACK0 ManagedKukaFriThunk::CreateCallBack0(Parameter0Delegate ^delgate0)
{
    delegateHandles->Add(GCHandle::Alloc(delgate0));
    IntPtr ip = Marshal::GetFunctionPointerForDelegate(delgate0);
    FRICALLBACK0 pfControl = static_cast<FRICALLBACK0>(ip.ToPointer());
    return pfControl;
}

/// <summary> Helper function to create a single parameter callback. </summary>
/// <param name="delgate1">Call back function </param>
FRICALLBACKPD1 ManagedKukaFriThunk::CreateCallBackPD1(ParameterPD1Delegate ^delgate1)
{
    delegateHandles->Add(GCHandle::Alloc(delgate1));
    IntPtr ip = Marshal::GetFunctionPointerForDelegate(delgate1);
    FRICALLBACKPD1 pfControl = static_cast<FRICALLBACKPD1>(ip.ToPointer());
    return pfControl;
}

/// <summary> Helper function to create a single parameter callback with a array buffer pointers. </summary>
/// <param name="delgateD1">Call back function </param>
FRICALLBACKPPD1 ManagedKukaFriThunk::CreateCallBackPPD1(ParameterPPD1Delegate ^delgateD1)
{
    delegateHandles->Add(GCHandle::Alloc(delgateD1));
    IntPtr ip = Marshal::GetFunctionPointerForDelegate(delgateD1);
    FRICALLBACKPPD1 pfControl = static_cast<FRICALLBACKPPD1>(ip.ToPointer());
    return pfControl;
}

/// <summary> Helper function to create a 4 parameter callback. </summary>
/// <param name="delgate4">Call back function </param>
FRICALLBACKPD4 ManagedKukaFriThunk::CreateCallBackPD4(ParameterPD4Delegate ^delgate4)
{
    delegateHandles->Add(GCHandle::Alloc(delgate4));
    IntPtr ip = Marshal::GetFunctionPointerForDelegate(delgate4);
    FRICALLBACKPD4 pfControl = static_cast<FRICALLBACKPD4>(ip.ToPointer());
    return pfControl;
}

/// <summary> Create a unmanaged call back with 1 double * and 1 int parameter </summary>
/// <param name="managedApi">delegate for callback</param>
FRICALLBACKPD1I1 ManagedKukaFriThunk::CreateCallBackPD1I1(ParameterPD1I1Delegate ^delgatePD1I1)
{
    delegateHandles->Add(GCHandle::Alloc(delgatePD1I1));
    IntPtr ip = Marshal::GetFunctionPointerForDelegate(delgatePD1I1);
    FRICALLBACKPD1I1 pfControl = static_cast<FRICALLBACKPD1I1>(ip.ToPointer());
    return pfControl;
}


/// <summary> Initializes the FRI connection. </summary>
/// <returns> Status code.  0 on no error. </returns>
int ManagedKukaFriThunk::InitializeFRI()
{
    return managedFRIConnection->InitializeFRI();
}

/// <summary> Shut down the FRI server. </summary>
/// <returns> Status code.  0 on no error. </returns>
int ManagedKukaFriThunk::CloseFRI()
{
    return managedFRIConnection->CloseFRI();
}

/// <summary> Wait for an FRI client to connect. </summary>
/// <returns> Status code.  0 on no error. </returns>
int ManagedKukaFriThunk::WaitOnFRIConnect()
{
    return managedFRIConnection->WaitOnFRIConnect();
}

/// <summary> Wait on a new FRI request. </summary>
/// <param name="currentPosition"> Current measured position. </param>
/// <returns> Status code.  0 on no error. </returns>
int ManagedKukaFriThunk::GetFRIRequest(double **ppRegisters)
{
    return managedFRIConnection->GetFRIRequest(ppRegisters);
}

/// <summary> Send a response to the FRI request, but take no control actions in it. </summary>
/// <returns> Status code.  0 on no error. </returns>
int ManagedKukaFriThunk::DoNothing()
{
    return managedFRIConnection->DoNothing();
}

/// <summary> Execute joint position control. </summary>
/// <param name="jointPosition"> Array of joint positions. </param>
/// <returns> Status code.  0 on no error. </returns>
int ManagedKukaFriThunk::DoPositionControl(double *jointPosition)
{
    return managedFRIConnection->DoPositionControl(jointPosition);
}

/// <summary> Execute joint impedance control. </summary>
/// <param name="jointPosition"> Array of joint positions. </param>
/// <param name="jointStiffness"> Array of joint stiffness values. </param>
/// <param name="jointDamping"> Array of joint damping values. </param>
/// <param name="jointAdditionalTorque"> Array of addition joint torque values. </param>
/// <returns> Status code.  0 on no error. </returns>
int ManagedKukaFriThunk::DoJointImpedanceControl(
    double *jointPosition,
    double *jointStiffness,
    double *jointDamping,
    double *jointAdditionalTorque)
{
    return managedFRIConnection->DoJointImpedanceControl(jointPosition, jointStiffness, jointDamping, jointAdditionalTorque);
}

/// <summary> Execute Cartesian impedance control. </summary>
/// <param name="cartesianPosition"> Array indicating desired cartesian position relative to base frame. </param>
/// <param name="cartesianStiffness"> Array of Cartesian stiffness values. </param>
/// <param name="cartesianDamping"> Array of Cartesian damping values. </param>
/// <param name="additionalTCPForceTorque"> Array of additional TCP force torque values. </param>
int ManagedKukaFriThunk::DoCartesianImpedanceControl(
    double *cartesianPosition,
    double *cartesianStiffness,
    double *cartesianDamping,
    double *additionalTCPForceTorque)
{
    return managedFRIConnection->DoCartesianImpedanceControl(cartesianPosition, cartesianStiffness, cartesianDamping, additionalTCPForceTorque);
}

/// <summary> Execute Cartesian impedance control using KRL motions. </summary>
/// <param name="cartesianPosition"> Array indicating desired cartesian position relative to base frame. </param>
/// <param name="sequenceNumber"> Sequence number of the command. </param>
int ManagedKukaFriThunk::DoKrlCartesianImpedanceControl(double *cartesianPosition, int sequenceNumber)
{
	return managedFRIConnection->DoKrlCartesianImpedanceControl(cartesianPosition, sequenceNumber);
}
